#include <librealsense2/rs.hpp>
#include <iostream>
#include <iomanip>

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

int main(int argc, char * argv[]) try
{
    // 
    rs2::pipeline pipe;
    // Create a configuration for configuring the pipeline with a non default profile
    rs2::config cfg;
    // Add pose stream
    cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
    cfg.enable_stream(RS2_STREAM_FISHEYE,1, RS2_FORMAT_Y8);
    cfg.enable_stream(RS2_STREAM_FISHEYE,2, RS2_FORMAT_Y8);

    // Start pipeline with chosen configuration
    pipe.start(cfg);
    rs2::pipeline_profile pipeline_profile = pipe.get_active_profile();
    rs2::stream_profile fisheye_left_profile = pipeline_profile.get_stream(RS2_STREAM_FISHEYE,1);
    rs2::stream_profile fisheye_right_profile = pipeline_profile.get_stream(RS2_STREAM_FISHEYE,2);
    rs2::stream_profile imu_profile = pipeline_profile.get_stream(RS2_STREAM_POSE);

    rs2_intrinsics intrinsics_left = fisheye_left_profile.as<rs2::video_stream_profile>().get_intrinsics();
    rs2_intrinsics intrinsics_right = fisheye_right_profile.as<rs2::video_stream_profile>().get_intrinsics();
    rs2_extrinsics extrinsics_left = fisheye_left_profile.get_extrinsics_to(imu_profile);
    rs2_extrinsics extrinsics_right = fisheye_right_profile.get_extrinsics_to(imu_profile);


    std::cout<<"the extrisics of fisheye1 to imu: "<<std::endl;
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            std::cout<<extrinsics_left.rotation[i*3+j]<<"  ";
        }

        std::cout<<extrinsics_left.translation[i]<<std::endl;
    }
    std::cout<<std::endl;
    std::cout<<"the extrisics of fisheye2 to imu: "<<std::endl;
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            std::cout<<extrinsics_right.rotation[i*3+j]<<"  ";
        }

        std::cout<<extrinsics_right.translation[i]<<std::endl;
    }
    std::cout<<std::endl;


    cv::Mat coeffleft(1,4,CV_64F);
    std::cout<<"the instrinsics of distror coefficent of left fisheye"<<std::endl;;
    for(int i=0;i<4;i++)
    {
        coeffleft.at<double>(0,i)=intrinsics_left.coeffs[i];
    }
    std::cout<<coeffleft<<std::endl;
    std::cout<<std::endl;
    cv::Mat coeffright(5,1,CV_32F);
    std::cout<<"the instrinsics of distror coefficent of right fisheye"<<std::endl;;
    for(int i=0;i<4;i++)
    {
        coeffright.at<double>(i,0)=intrinsics_left.coeffs[i];
    }
    coeffright.at<double>(4,0)=0.0;
    std::cout<<coeffright<<std::endl;

    std::cout<<"the intrinsics mat of the left fisheye  "<<std::endl;
    cv::Mat cameraMatrixleft = cv::Mat::zeros(3, 3, CV_64F);
    cameraMatrixleft.at<double>(0,0)=intrinsics_left.fx;
    cameraMatrixleft.at<double>(0,2)=intrinsics_left.ppx;
    cameraMatrixleft.at<double>(1,1)=intrinsics_left.fy;
    cameraMatrixleft.at<double>(1,2)=intrinsics_left.ppy;
    cameraMatrixleft.at<double>(2,2)=1.0;
    std::cout<<cameraMatrixleft<<std::endl;

    std::cout<<"the intrinsics mat of the right fisheye  "<<std::endl;
    cv::Mat cameraMatrixright = cv::Mat::zeros(3, 3, CV_64F);
    cameraMatrixright.at<double>(0,0)=intrinsics_right.fx;
    cameraMatrixright.at<double>(0,2)=intrinsics_right.ppx;
    cameraMatrixright.at<double>(1,1)=intrinsics_right.fy;
    cameraMatrixright.at<double>(1,2)=intrinsics_right.ppy;
    cameraMatrixright.at<double>(2,2)=1.0;
    std::cout<<cameraMatrixright<<std::endl;

    int countl=0,countr=0;
    // Main loop
    while (true)
    {
        // Wait for the next set of frames from the camera
        auto frames = pipe.wait_for_frames();
        // Get a frame from the pose stream
        auto f = frames.first_or_default(RS2_STREAM_POSE);
        auto pose_data = f.as<rs2::pose_frame>().get_pose_data();



        // Print the x, y, z values of the translation, relative to initial position
        std::cout << "\r" << "Device Position: " << std::setprecision(3) << std::fixed << pose_data.translation.x << " " <<
                     pose_data.translation.y << " " << pose_data.translation.z << " (meters)";

        rs2::video_frame image_left = frames.get_fisheye_frame(1);
        rs2::video_frame image_right =frames.get_fisheye_frame(2);
        if (!image_left || !image_right)
            break;

        cv::Mat cv_image_left(cv::Size(848, 800), CV_8U, (void*)image_left.get_data(), cv::Mat::AUTO_STEP);
        cv::Mat cv_image_right(cv::Size(848, 800), CV_8U, (void*)image_right.get_data(), cv::Mat::AUTO_STEP);
        cv::Mat leftimg,rightimg;
        cv::cvtColor(cv_image_left,cv_image_left,cv::COLOR_GRAY2BGR);
        cv::imshow("raw left",cv_image_left);
        std::cout<<cameraMatrixleft<<std::endl;
        std::cout<<coeffleft<<std::endl;
        cv::Mat map1,map2;
        //cv::fisheye::undistortImage(cv_image_left,leftimg,cameraMatrixleft,coeffleft);
        cv::initUndistortRectifyMap(cameraMatrixleft, coeffleft, cv::Mat(),
                                    cv::getOptimalNewCameraMatrix(cameraMatrixleft, coeffleft, 
                                                                  cv_image_left.size(), 1, cv_image_left.size(), 0),
                                   cv_image_left.size(), CV_16SC2, leftimg, map2);

        //cv::undistort(cv_image_right,rightimg,cameraMatrix,coeff,cameraMatrix);

        cv::imshow("left", leftimg);
        //cv::imwrite("./imgs/left/"+std::to_string(countl++)+".jpg",leftimg);
        //cv::imwrite("./imgs/right/"+std::to_string(countr++)+".jpg",rightimg);
        cv::waitKey(5);
    }

    return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}
